function [theta_next, i_next, w_next] = Torque_mode(Kg, w_now, uc, Ka, Im, Bm, G_sel, m, L, g, theta_now, n)
%Author:唐友康 19374028
%Ke为感应电动势常数，w_now为当前角速度，ua为电枢电压，Ra为电枢电阻，i_now为当前电流，La为电枢电感
%Ka为转矩常数，Im为等效惯量，Bm为等效阻尼，G_sel为重力选择标志位
%m为连杆质量，L为连杆质心到转子距离, g为重力加速度, theta_now为当前角度,n为传动比

tm = Ka*Kg*uc;
i_next = Kg * uc;
dw = Get_Beta(Im, w_now, Bm, tm, G_sel, m, L, g, theta_now, n);
w_next = w_now + dw*0.0001;
theta_next = theta_now + w_now*0.0001;